#include "PhysicalScene.h"

#include "Containers/Tuple.h"
#include "Containers/Vector.h"

#include "RigidBody.h"

#include "Collider/Collider.h"
#include "Collider/SphereCollider.h"
#include "Collider/BoxCollider.h"

void RPhysicalScene::Tick(float DeltaTime)
{
    AccumulatedTime+=DeltaTime;

    while(AccumulatedTime>TickPeriod)
    {
        TickOneStep(TickPeriod);
        AccumulatedTime-=TickPeriod;
    }
}

void RPhysicalScene::TickOneStep(float DeltaTime)
{
    //1.粗粒度碰撞检测，快速排除不可能碰撞的物体
    BroadPhaseCollisionDetection();

    //2.精细碰撞检测，检测碰撞的物体  
    NarrowPhaseCollisionDetection();

    //3.处理碰撞结果
    HandleCollisionResult();

    //4.更新所有刚体的状态
    UpdateAllRigidBodyState(DeltaTime);
}

void RPhysicalScene::UpdateAllRigidBodyState(float DeltaTime)
{
    for(auto& Body: RigidBodies)
    {
        UpdateRigidBodyState(Body,DeltaTime);
    }
}

void RPhysicalScene::UpdateRigidBodyState(TSharedObjectPtr<RRigidBody> InBody, float DeltaTime)
{
    //使用半隐式欧拉法更新刚体的状态
    //v(t+dt) = v(t) + a(t) * dt
    //x(t+dt) = x(t) + v(t+dt) * dt

    //计算加速度
    SVec3 Acceleration = InBody->Force / InBody->Mass;

    //更新线速度
    InBody->LinearVelocity+=Acceleration*DeltaTime;

    //更新位置
    InBody->SetWorldPosition(InBody->GetWorldPosition()+InBody->LinearVelocity*DeltaTime);

    //计算角加速度
    SVec3 AngularAcceleration = InBody->InertiaTensorInverse * InBody->Torque;

    //更新角速度
    InBody->AngularVelocity+=AngularAcceleration*DeltaTime;

    //更新旋转
    SQuat Rotation = SQuat(InBody->AngularVelocity,DeltaTime);

    InBody->SetWorldRotation(Rotation*InBody->GetWorldRotation());

    //清空受力和力矩
    InBody->Force = SVec3(0,0,0);

    InBody->Torque = SVec3(0,0,0);

}

void RPhysicalScene::BroadPhaseCollisionDetection()
{
   
    //1.1将所有碰撞体按照包围盒进行排序
    enum class EPointType
    {
        Min=0,
        Max=1,
    };

    struct SSortedPoint
    {
        float Value;
        EPointType Type;

        TSharedObjectPtr<RCollider> Collider;
        //碰撞器在碰撞器列表中的索引
        int32_t IndexInColliderArray;
    };

    TVector<SSortedPoint> SortedPointsX;
    TVector<SSortedPoint> SortedPointsY;  
    TVector<SSortedPoint> SortedPointsZ; 
    
    SortedPointsX.Reserve(Colliders.Num()*2);
    SortedPointsY.Reserve(Colliders.Num()*2);
    SortedPointsZ.Reserve(Colliders.Num()*2);

    //插入所有碰撞体的包围盒的最大最小点
    for(int32_t i=0;i<Colliders.Num();i++)
    {
        TSharedObjectPtr<RCollider> Collider = Colliders[i];
        SBoundingBox AABB = Collider->GetWorldAABB();

        SortedPointsX.Add({AABB.Min.X,EPointType::Min,Collider,i});
        SortedPointsX.Add({AABB.Max.X,EPointType::Max,Collider,i});

        SortedPointsY.Add({AABB.Min.Y,EPointType::Min,Collider,i});
        SortedPointsY.Add({AABB.Max.Y,EPointType::Max,Collider,i});

        SortedPointsZ.Add({AABB.Min.Z,EPointType::Min,Collider,i});
        SortedPointsZ.Add({AABB.Max.Z,EPointType::Max,Collider,i});

    }

    //从小到大排序
    //如果值相同，Min点在前，Max点在后,这样可以保证相同X的情况下被判定为相交
    auto SortFunc = [](const SSortedPoint& A,const SSortedPoint& B)
    {
        if(A.Value==B.Value)
        {
            return A.Type<B.Type;
        }
        return A.Value<B.Value;
    };

    SortedPointsX.Sort(SortFunc);
    SortedPointsY.Sort(SortFunc);
    SortedPointsZ.Sort(SortFunc);

    //生成列表用于快速获取碰撞器在排序列表中的位置
    TVector<int32_t> ColliderMinXIndex;
    TVector<int32_t> ColliderMaxXIndex;
    TVector<int32_t> ColliderMinYIndex;
    TVector<int32_t> ColliderMaxYIndex;
    TVector<int32_t> ColliderMinZIndex;
    TVector<int32_t> ColliderMaxZIndex;

    ColliderMinXIndex.Resize(Colliders.Num());
    ColliderMaxXIndex.Resize(Colliders.Num());
    ColliderMinYIndex.Resize(Colliders.Num());
    ColliderMaxYIndex.Resize(Colliders.Num());
    ColliderMinZIndex.Resize(Colliders.Num());
    ColliderMaxZIndex.Resize(Colliders.Num());

    for(int32_t i=0;i<SortedPointsX.Num();i++)
    {
        if(SortedPointsX[i].Type==EPointType::Min)
        {
            ColliderMinXIndex[SortedPointsX[i].IndexInColliderArray]=i;
        }
        else
        {
            ColliderMaxXIndex[SortedPointsX[i].IndexInColliderArray]=i;
        }
    }

    for(int32_t i=0;i<SortedPointsY.Num();i++)
    {
        if(SortedPointsY[i].Type==EPointType::Min)
        {
            ColliderMinYIndex[SortedPointsY[i].IndexInColliderArray]=i;
        }
        else
        {
            ColliderMaxYIndex[SortedPointsY[i].IndexInColliderArray]=i;
        }
    }

    for(int32_t i=0;i<SortedPointsZ.Num();i++)
    {
        if(SortedPointsZ[i].Type==EPointType::Min)
        {
            ColliderMinZIndex[SortedPointsZ[i].IndexInColliderArray]=i;
        }
        else
        {
            ColliderMaxZIndex[SortedPointsZ[i].IndexInColliderArray]=i;
        }
    }

    //1.2 找到可能碰撞的物体对
    BroadPhaseResult.Resize(Colliders.Num());

    for(int32_t A=0;A<Colliders.Num();A++)
    {
        //检测X轴
        TVector<int32_t> CollidesX;

        for(int32_t i=ColliderMinXIndex[A]+1;i<ColliderMaxXIndex[A];i++)
        {
            CollidesX.Add(SortedPointsX[i].IndexInColliderArray);
        }

        //检测Y轴
        TVector<int32_t> CollidesY;

        for(int32_t i=ColliderMinYIndex[A]+1;i<ColliderMaxYIndex[A];i++)
        {
            CollidesY.Add(SortedPointsY[i].IndexInColliderArray);
        }

        //检测Z轴
        TVector<int32_t> CollidesZ;

        for(int32_t i=ColliderMinZIndex[A]+1;i<ColliderMaxZIndex[A];i++)
        {
            CollidesZ.Add(SortedPointsZ[i].IndexInColliderArray);
        }

        //取交集
        TVector<int32_t>& Collides = BroadPhaseResult[A];
        for(int32_t B: CollidesX)
        {
            if(CollidesY.Contains(B) && CollidesZ.Contains(B))
            {
                Collides.Add(B);
            }
        }
    }
}

void RPhysicalScene::NarrowPhaseCollisionDetection()
{
    for(int32_t A=0;A<Colliders.Num();A++)
    {
        TSharedObjectPtr<RCollider> ColliderA = Colliders[A];

        for(int32_t B: BroadPhaseResult[A])
        {
            //防止重复检测同一对碰撞体
            if(B<=A)
            {
                continue;
            }

            TSharedObjectPtr<RCollider> ColliderB = Colliders[B];

            //检测碰撞
            if(!NeedCollisionDetect(ColliderA,ColliderB))
            {
                continue;
            }

            auto Result=CollisionDetect(ColliderA,ColliderB);

            if(!Result.HasValue())
            {
                //没有发生碰撞
                continue;
            }

            //发生碰撞
            NarrowPhaseResult.Add(Result.GetValue());
        }
    }

}

bool RPhysicalScene::NeedCollisionDetect(TSharedObjectPtr<RCollider> ColliderA, TSharedObjectPtr<RCollider> ColliderB)
{
    //TODO: 业务上的碰撞检测
    return true;
}

TOptional<RPhysicalScene::SCollisionResult> RPhysicalScene::CollisionDetect(TSharedObjectPtr<RCollider> ColliderA, TSharedObjectPtr<RCollider> ColliderB)
{
    //根据碰撞体形状进行碰撞检测
    if(ColliderA->IsA<RSphereCollider>())
    {
        if(ColliderB->IsA<RSphereCollider>())
        {
            return CollisionDetectSphereSphere(
                TSharedObjectPtr<RSphereCollider>(ColliderA),
                TSharedObjectPtr<RSphereCollider>(ColliderB)
            );
        }
        else if(ColliderB->IsA<RBoxCollider>())
        {
            return CollisionDetectSphereBox(
                TSharedObjectPtr<RSphereCollider>(ColliderA),
                TSharedObjectPtr<RBoxCollider>(ColliderB)
            );
        }
        else
        {
            //未实现的碰撞检测
            assert(false);
        }
    }
    else if(ColliderA->IsA<RBoxCollider>())
    {
        if(ColliderB->IsA<RSphereCollider>())
        {
            return CollisionDetectSphereBox(
                TSharedObjectPtr<RSphereCollider>(ColliderB),
                TSharedObjectPtr<RBoxCollider>(ColliderA)
            );
        }
        else if(ColliderB->IsA<RBoxCollider>())
        {
            return CollisionDetectBoxBox(
                TSharedObjectPtr<RBoxCollider>(ColliderA),
                TSharedObjectPtr<RBoxCollider>(ColliderB)
            );
        }
        else
        {
            //未实现的碰撞检测
            assert(false);
        }
    }
    else
    {
        //未实现的碰撞检测
        assert(false);
    }

    assert(false&& "未实现的碰撞检测");
    return TOptional<RPhysicalScene::SCollisionResult>();

}

TOptional<RPhysicalScene::SCollisionResult> RPhysicalScene::CollisionDetectSphereSphere(TSharedObjectPtr<RSphereCollider> SphereA, TSharedObjectPtr<RSphereCollider> SphereB)
{
    SVec3 ACenter=SphereA->GetCenterWorldPosition();
    SVec3 BCenter=SphereB->GetCenterWorldPosition();

    float Distance = (ACenter-BCenter).GetLength();

    float Threshold = SphereA->GetRadius()+SphereB->GetRadius();

    if(Distance>Threshold)
    {
        //未发生碰撞
        return TOptional<RPhysicalScene::SCollisionResult>();
    }

    //发生碰撞
    RPhysicalScene::SCollisionResult Result;
    Result.ColliderA = SphereA;
    Result.ColliderB = SphereB;
    Result.ContactPoint = (ACenter+BCenter)*0.5f;
    Result.Normal = (ACenter-BCenter).ToNormalized();
    Result.Penetration = Threshold-Distance;

    return Result;
}

TOptional<RPhysicalScene::SCollisionResult> RPhysicalScene::CollisionDetectSphereBox(TSharedObjectPtr<RSphereCollider> Sphere, TSharedObjectPtr<RBoxCollider> Box)
{
    //TODO: 球与盒子的碰撞检测
    return TOptional<RPhysicalScene::SCollisionResult>();
}

TOptional<RPhysicalScene::SCollisionResult> RPhysicalScene::CollisionDetectBoxBox(TSharedObjectPtr<RBoxCollider> BoxA, TSharedObjectPtr<RBoxCollider> BoxB)
{
    //TODO 盒子与盒子的碰撞检测
    return TOptional<RPhysicalScene::SCollisionResult>();
}

void RPhysicalScene::HandleCollisionResult()
{
    for(auto& Result: NarrowPhaseResult)
    {
        if(AllowOverlap(Result.ColliderA,Result.ColliderB))
        {
            //允许重叠
            continue;
        }

        auto BodyA = GetRigidBodyByCollider(Result.ColliderA);
        auto BodyB = GetRigidBodyByCollider(Result.ColliderB);

        if(!BodyA.IsValid() || !BodyB.IsValid())
        {
            //没有刚体,不处理
            continue;
        }

        //如果只有一个刚体,那么反弹另一个刚体
        //保持动量守恒
        //动能不守恒，因为碰撞会产生热量，动能会转化为热能
        if(BodyA.IsValid() && !BodyB.IsValid())
        {
            //TODO 由于动能损失，需要引入恢复系数
            BodyA->LinearVelocity = -BodyA->LinearVelocity;
            BodyA->AngularVelocity = -BodyA->AngularVelocity;
        }
        else if(!BodyA.IsValid() && BodyB.IsValid())
        {
            //TODO 由于动能损失，需要引入恢复系数
            BodyB->LinearVelocity = -BodyB->LinearVelocity;
            BodyB->AngularVelocity = -BodyB->AngularVelocity;
        }
        else
        {
            //两个刚体都存在
            //TODO: 处理两个刚体的碰撞
            //暂时让两个刚体停止
            BodyA->LinearVelocity = SVec3(0,0,0);
            BodyA->AngularVelocity = SVec3(0,0,0);

            BodyB->LinearVelocity = SVec3(0,0,0);
            BodyB->AngularVelocity = SVec3(0,0,0);
        }

    };

}

bool RPhysicalScene::AllowOverlap(TSharedObjectPtr<RCollider> ColliderA, TSharedObjectPtr<RCollider> ColliderB)
{
    //TODO: 业务上的重叠检测
    return false;
}

TSharedObjectPtr<RRigidBody> RPhysicalScene::GetRigidBodyByCollider(TSharedObjectPtr<RCollider> InCollider)
{
    return InCollider->GetRigidBody();
}
